#include <math.h>
#include "misc.h"

/***************************************
 * 函数功能: 求角度差值 单位 rad
 * 参数: des 目标角度；cur 当前角度
 * 返回值: 正数表示正方向旋转，负数表示反方向旋转
 * *************************************/
float angleDifference(float des,float cur)
{
    float d1,d2;

    d1 = des-cur;
    d2 = 2*M_PI-fabs(d1);

    if(d1 > 0)
        d2 *= -1;
    
    if(fabs(d1)>fabs(d2))
        return d2;
    else
        return d1;
}
/***************************************
 * 函数功能: 约束
 * 参数: value 目标值指针；min 最小值；max 最大值
 * 返回值: 正数表示正方向旋转，负数表示反方向旋转
 * *************************************/
void fbound(float *value,float min,float max)
{
    *value = *value > max ? max : *value;
    *value = *value < min ? min : *value;
}